/*
 *  ColladaToProtoBuffer.cpp
 *  ColladaToProtoBuffer
 *
 *  Created by Micah Pearlman on 12/11/08.
 *  Copyright 2008 Monk Games. All rights reserved.
 *
 */

#include "ColladaToProtoBuffer.h"


//DOM API includes
#include <dae.h>
#include <dom/domConstants.h>
#include <dom/domImage.h>

#include <fstream>

// wild magic library includes
#include "Wm4Vector3.h"
#include "Wm4Matrix3.h"
#include "Wm4Quaternion.h"


using namespace std;
using namespace ::collada_to_protobuf;
using namespace Wm4;


bool ColladaToProtoBuffer::init() {
	
	//CONFIGURE YOUR CONDITIONER
	
	setNumInputs(1,false);
	
	//ADD USER OPTIONS
	
	//e.g. boolean option, see also addFloatOption(...), addStringOption(...)
	addBoolOption("myBoolOption", "full name", "write description in here.", true);
	
	dom_ = _dae->getRoot( this->getName() );
	
	return true;
}

int ColladaToProtoBuffer::execute() {
	
	//QUERY THE DAE DATABASE AND PERFORM THE MANIPULATION OF COLLADA-DATA
	//Use the _dae member of type DAE*
	
	//See http://www.collada.org/mediawiki/index.php/DOM_runtime_database
	//for a guide how to use the DOM runtime database
	
	// Verify that the version of the protobuf library that we linked against is
	// compatible with the version of the headers we compiled against.
	GOOGLE_PROTOBUF_VERIFY_VERSION;
	
	
	ProcessPhysicsMaterials();
	ProcessPhysicsModels();
	ProcessPhysicsScenes();

	for( int i = 0; i < this->getNumInputs(); i++ )
		LibLoader::printExecutionMessage( std::string( "Input: " ) + this->getInput(i) );
	for( int i = 0; i < this->getNumInputs(); i++ )		
		LibLoader::printExecutionMessage( std::string( "Output: " ) + this->getOutput(i) );		
	LibLoader::printExecutionMessage( std::string( "Name: " ) + this->getName() );	
	LibLoader::printExecutionMessage( std::string( "Base Name: " ) + this->getBaseName() );	
		
	// write out the proto buffers to disk
	if( physics_material_library_.IsInitialized() )
	{
		std::string path = this->getOutput(0) + std::string("_physics_material_library.pb");
		LibLoader::printExecutionMessage( std::string( "Writing File: " ) + path );
		fstream output(path.c_str(), ios::out | ios::binary );
		if (!physics_material_library_.SerializePartialToOstream(&output)) {
//			cerr << "Failed to write protobuffer." << endl;
		}
	}
	else 
	{
		LibLoader::printExecutionMessage( std::string( "protobuf Init Errors for physics_material_library_: " ) + physics_material_library_.InitializationErrorString() );
	}
	
	if( physics_model_library_.IsInitialized() )
	{
		std::string path = this->getOutput(0) + std::string("_physics_model_library.pb");
		LibLoader::printExecutionMessage( std::string( "Writing File: " ) + path );
		fstream output(path.c_str(), ios::out | ios::binary );
		if (!physics_model_library_.SerializePartialToOstream(&output)) {
			//			cerr << "Failed to write protobuffer." << endl;
		}
	}
	else 
	{
		LibLoader::printExecutionMessage( std::string( "protobuf Init Errors for physics_model_library_: " ) + physics_model_library_.InitializationErrorString() );
	}
	
	if( physics_scene_library_.IsInitialized() )
	{
		std::string path = this->getOutput(0) + std::string("_physics_scene_library.pb");
		LibLoader::printExecutionMessage( std::string( "Writing File: " ) + path );
		fstream output(path.c_str(), ios::out | ios::binary );
		if (!physics_scene_library_.SerializePartialToOstream(&output)) {
			//			cerr << "Failed to write protobuffer." << endl;
		}
	}
	else 
	{
		LibLoader::printExecutionMessage( std::string( "protobuf Init Errors for physics_scene_library_: " ) + physics_scene_library_.InitializationErrorString() );
	}
	
	return EXIT_SUCCESS;
}

bool ColladaToProtoBuffer::ProcessRigidBodies()
{
	
	return true;	// success
}

bool ColladaToProtoBuffer::ProcessPhysicsMaterials()
{
	//domLibrary_physics_materials_Array& physicsMaterialsArray = dom_->getLibrary_physics_materials_array();
	//domLibrary_physics_materials* materialsLib = physicsMaterialsArray[0];
	//materialsLib->
	const unsigned int iNumElements = _dae->getDatabase()->getElementCount(NULL, COLLADA_TYPE_PHYSICS_MATERIAL, NULL);
	
	domPhysics_material *pPhysicsMaterial = NULL;  
	
	for ( unsigned int i = 0; i < iNumElements; i++ ) {
		
		_dae->getDatabase()->getElement( (daeElement**)&pPhysicsMaterial, i, NULL, COLLADA_TYPE_PHYSICS_MATERIAL, NULL );
		LibLoader::printExecutionMessage( std::string( "Processing Physics Material: " ) + pPhysicsMaterial->getID() );
		
		// create a new material in the physics material library
		physics_material *material_pb = physics_material_library_.add_physics_materials_();
		
		// create the attribute...
		Attributes* attr = material_pb->mutable_attr_();
		attr->set_id_( pPhysicsMaterial->getID() );
		attr->set_hash_id_( StringHash( attr->id_() ) );	// hash id for quick look up
		
		// get the common technique
		domPhysics_material::domTechnique_common* material_common_dae 
			= (domPhysics_material::domTechnique_common*)pPhysicsMaterial->getChild( "technique_common" );
		
		// set values
		material_pb->set_dynamic_friction_( material_common_dae->getDynamic_friction()->getValue() );
		material_pb->set_static_friction_( material_common_dae->getStatic_friction()->getValue() );
		material_pb->set_restitution_( material_common_dae->getRestitution()->getValue() );
	}
	
	return true;	// success
}

bool ColladaToProtoBuffer::ProcessConstraints()
{
	return true;
}

bool ColladaToProtoBuffer::ProcessPhysicsModels()
{
	const unsigned int iNumElements = _dae->getDatabase()->getElementCount(NULL, COLLADA_TYPE_PHYSICS_MODEL, NULL);
	
	domPhysics_model *pDomPhysicsModel = NULL;  
	
	for ( unsigned int i = 0; i < iNumElements; i++ ) 
	{
		
		_dae->getDatabase()->getElement( (daeElement**)&pDomPhysicsModel, i, NULL, COLLADA_TYPE_PHYSICS_MODEL, NULL );
		LibLoader::printExecutionMessage( std::string( "Processing Physics Model: " ) + pDomPhysicsModel->getID() );
		
		// create a new physics model in the physics model library
		physics_model *pPbPhysicsModel = physics_model_library_.add_physics_models_();
		
		// create the attribute...
		Attributes* attr = pPbPhysicsModel->mutable_attr_();
		attr->set_id_( pDomPhysicsModel->getID() );
		attr->set_hash_id_( StringHash( attr->id_() ) );	// hash id for quick look up
		
		// go through the physics model rigid bodies...
		domRigid_body_Array& rbArray = pDomPhysicsModel->getRigid_body_array();
		for ( unsigned int p = 0; p < rbArray.getCount(); p++ ) 
		{
		
			domRigid_body* domRb = rbArray.get( p );
			rigid_body *pPbRigidBody = pPbPhysicsModel->add_rigid_bodies_(); 
			LibLoader::printExecutionMessage( std::string( "\tProcessing Rigid Body: " ) + domRb->getAttribute( "sid" ) );
			
			// set th rigid body attribute info
			attr = pPbRigidBody->mutable_attr_();
			attr->set_id_( domRb->getAttribute( "sid" ) );
			attr->set_hash_id_( StringHash( attr->id_() ) );	// hash id for quick look up
			
			// get the common technique
			domRigid_body::domTechnique_common* pDomRbCommon 
				= (domRigid_body::domTechnique_common*)domRb->getChild( "technique_common" );
				
			if( pDomRbCommon->getDynamic() )
				pPbRigidBody->set_dynamic_( pDomRbCommon->getDynamic()->getValue() );
				
			if( pDomRbCommon->getMass() )
				pPbRigidBody->set_mass_( pDomRbCommon->getMass()->getValue() );
				
			if( pDomRbCommon->getMass_frame() )
			{
				domTranslate_Array &tranArray = pDomRbCommon->getMass_frame()->getTranslate_array();
				domRotate_Array &rotArray = pDomRbCommon->getMass_frame()->getRotate_array();
				
				domFloat3 tran = tranArray.get( 0 )->getValue();	// bugbug? just getting the first...
				domFloat4 rot = rotArray.get( 0 )->getValue();		// bugbug? just getting the first...
				
				mass_frame* pMassFrame = pPbRigidBody->mutable_mass_frame_();
				pMassFrame->mutable_translate_()->set_x( tran.get(0) );
				pMassFrame->mutable_translate_()->set_y( tran.get(1) );
				pMassFrame->mutable_translate_()->set_z( tran.get(2) );

				pMassFrame->mutable_rotate_()->set_x( rot.get(0) );
				pMassFrame->mutable_rotate_()->set_y( rot.get(1) );
				pMassFrame->mutable_rotate_()->set_z( rot.get(2) );
				pMassFrame->mutable_rotate_()->set_w( rot.get(3) );
			}

			if( pDomRbCommon->getInertia() )
			{
				domTargetableFloat3Ref rot = pDomRbCommon->getInertia();
				
				vec3f* inertia = pPbRigidBody->mutable_inertia_();
				inertia->set_x( rot->getValue().get(0) );
				inertia->set_y( rot->getValue().get(1) );
				inertia->set_z( rot->getValue().get(2) );
			}
			
			// get the material...
			const domInstance_physics_materialRef pDomPhysicsMaterial = pDomRbCommon->getInstance_physics_material();
			if( pDomPhysicsMaterial )
			{
				std::string url = pDomPhysicsMaterial->getAttribute( "url" );
				
				if( url[0] == '#' )
					url.erase( 0, 1 );	// get rid of the # in the url
					
				Attributes *attr = pPbRigidBody->mutable_instance_physics_material_()->mutable_attr_();
				attr->set_id_( url );
				attr->set_hash_id_( StringHash( url ) );	// hash id for quick look up

				LibLoader::printExecutionMessage( std::string( "\tProcessing Instance Physics Material: " ) + url );
			}
			
			// now go through and the shape(s)
			domRigid_body::domTechnique_common::domShape_Array &shapeArray = pDomRbCommon->getShape_array();
			for( int j = 0; j < shapeArray.getCount(); j++ )
			{
				domRigid_body::domTechnique_common::domShapeRef pDomShape = shapeArray.get( j );
				
				// create a shape
				shape* pPbShape = pPbRigidBody->add_shapes_();
				
				if( pDomShape->getPlane() )
				{
					domPlaneRef pDomPlane = pDomShape->getPlane();
					
					domFloat4 &equation = pDomPlane->getEquation()->getValue();
					
					plane* pPbPlane = pPbShape->mutable_plane_();
					vec4f* v = pPbPlane->mutable_equation_();
					v->set_x( equation.get( 0 ) );
					v->set_y( equation.get( 1 ) );
					v->set_z( equation.get( 2 ) );
					v->set_w( equation.get( 3 ) );
					
					LibLoader::printExecutionMessage( "\t-- Plane Shape" );
				}
				else if( pDomShape->getBox() )
				{
					domBoxRef pDomBox = pDomShape->getBox();
					domFloat3 &halfExtents = pDomBox->getHalf_extents()->getValue();
					
					box* pPbBox = pPbShape->mutable_box_();
					vec3f* v = pPbBox->mutable_half_extents_();
					v->set_x( halfExtents.get( 0 ) );
					v->set_y( halfExtents.get( 1 ) );
					v->set_z( halfExtents.get( 2 ) );
					
					LibLoader::printExecutionMessage( "\t-- Box Shape" );
				}
				else if( pDomShape->getSphere() )
				{
					domSphereRef pDomSphere = pDomShape->getSphere();
					const domFloat &radius = pDomSphere->getRadius()->getValue();
					
					sphere* pPbSphere = pPbShape->mutable_sphere_();
					pPbSphere->set_radius_( radius );
					
					LibLoader::printExecutionMessage( "\t-- Sphere Shape" );
				}
				else if( pDomShape->getInstance_geometry() )
				{
					domInstance_geometryRef pDomInstanceGeometry = pDomShape->getInstance_geometry();
					std::string url = pDomInstanceGeometry->getAttribute( "url" );
					
					if( url[0] == '#' )
						url.erase( 0, 1 );	// get rid of the # in the url
					
					instance_geometry *pPbInstanceGeo = pPbShape->mutable_instance_geometry_();
					
					Attributes* attr = pPbInstanceGeo->mutable_attr_();
					attr->set_id_( url );
					attr->set_hash_id_( StringHash( url ) );	// hash id for quick look up
					
					LibLoader::printExecutionMessage( "\t-- Instance Geometry Shape" );
				}
				else if(  pDomShape->getCylinder() )
				{
					domCylinderRef pDomCylinder = pDomShape->getCylinder();
					domFloat2 &radius = pDomCylinder->getRadius()->getValue();
					domFloat height = pDomCylinder->getHeight()->getValue();
					
					cylinder* pPbCylinder = pPbShape->mutable_cylinder_();
					vec2f *v = pPbCylinder->mutable_radius_();
					v->set_x( radius.get( 0 ) );
					v->set_y( radius.get( 1 ) );
					
					pPbCylinder->set_height_( height );
					
					LibLoader::printExecutionMessage( "\t-- Cylinder Shape" );
				}
				else if( pDomShape->getCapsule() )
				{
					domCapsuleRef pDomCapsule = pDomShape->getCapsule();
					domFloat2 &radius = pDomCapsule->getRadius()->getValue();
					domFloat height = pDomCapsule->getHeight()->getValue();
					
					capsule* pPbCapsule = pPbShape->mutable_capsule_();
					vec2f *v = pPbCapsule->mutable_radius_();
					v->set_x( radius.get( 0 ) );
					v->set_y( radius.get( 1 ) );
					
					pPbCapsule->set_height_( height );

					LibLoader::printExecutionMessage( "\t-- Capsule Shape" );					
				}
				
			} // for each shape
		} // for each rigid body
	} // for each physics model
	
	return true;	// success
}

bool ColladaToProtoBuffer::ProcessPhysicsScenes()
{
	const unsigned int iNumElements = _dae->getDatabase()->getElementCount(NULL, COLLADA_TYPE_PHYSICS_SCENE, NULL);
	
	domPhysics_scene *pDomPhysicsScene = NULL;  
	
	for ( unsigned int i = 0; i < iNumElements; i++ ) 
	{
		_dae->getDatabase()->getElement( (daeElement**)&pDomPhysicsScene, i, NULL, COLLADA_TYPE_PHYSICS_SCENE, NULL );
		physics_scene *pPbPhysicsScene = physics_scene_library_.add_physics_scenes_();
		
		// create the attribute...
		Attributes* attr = pPbPhysicsScene->mutable_attr_();
		attr->set_id_( pDomPhysicsScene->getID() );
		attr->set_hash_id_( StringHash( attr->id_() ) );	// hash id for quick look up
		
		// get technique common values
		domPhysics_scene::domTechnique_common* pDomSceneCommon 
			= (domPhysics_scene::domTechnique_common*)pDomPhysicsScene->getChild( "technique_common" );
		
		vec3f *v = pPbPhysicsScene->mutable_gravity_();
		if( _dae->getDatabase()->getElementCount(NULL, COLLADA_ELEMENT_GRAVITY, NULL) > 0 )	
		{
			domFloat3 gravity = pDomSceneCommon->getGravity()->getValue();
			
			v->set_x( gravity.get( 0 ) );
			v->set_y( gravity.get( 1 ) );		
			v->set_z( gravity.get( 2 ) );
		}
		else // dom doesn't have gravity defined so set default
		{
			v->set_x( 0.0f );
			v->set_y( 0.0f );
			v->set_z( -9.8f );
		}
		
		domFloat time_step = 1.0f/60.0f;
		if( _dae->getDatabase()->getElementCount(NULL, COLLADA_ELEMENT_TIME_STEP, NULL ) )
		{
			time_step = pDomSceneCommon->getTime_step()->getValue();
		}
		pPbPhysicsScene->set_time_step_( time_step );
		
		// go through each physics model instance
		domInstance_physics_model_Array &physicsModelArray = pDomPhysicsScene->getInstance_physics_model_array();
		for( int j = 0; j < physicsModelArray.getCount(); j++ )
		{ 
			domInstance_physics_modelRef pDomInstancePhysicsModel = physicsModelArray.get( j );
			instance_physics_model *pPbInstancePhysicsModel = pPbPhysicsScene->add_instance_physics_models_();
			
			// get the physics model url to intance
			std::string url = pDomInstancePhysicsModel->getAttribute( "url" );
			if( url[0] == '#' )
				url.erase( 0, 1 );	// get rid of the # in the url
				
			LibLoader::printExecutionMessage( std::string( "\tProcessing Instance Physics Model: " ) + url );
			
			Attributes *attr = pPbInstancePhysicsModel->mutable_url_();
			attr->set_id_( url );
			attr->set_hash_id_( StringHash( url ) );	// hash id for quick look up
			
			// go through each rigid body instance
			domInstance_rigid_body_Array &instanceRigidBodyArray = pDomInstancePhysicsModel->getInstance_rigid_body_array();
			for( int k = 0; k < instanceRigidBodyArray.getCount(); k++ )
			{
				domInstance_rigid_bodyRef pDomInstanceRigidBody = instanceRigidBodyArray.get( k );
				instance_rigid_body *pPbInstanceRigidBody = pPbInstancePhysicsModel->add_rigid_body_instances_();
				
				std::string body = pDomInstanceRigidBody->getAttribute( "body" );
				if( body[0] == '#' )
					body.erase( 0, 1 );	// get rid of the # in the url
								
				attr = pPbInstanceRigidBody->mutable_body_();
				attr->set_id_( body );
				attr->set_hash_id_( StringHash( body ) );	// hash id for quick look up
				
				std::string target = pDomInstanceRigidBody->getAttribute( "target" );
				if( target[0] == '#' )
					target.erase( 0, 1 );	// get rid of the # in the url
				
				attr = pPbInstanceRigidBody->mutable_target_();
				attr->set_id_( target );
				attr->set_hash_id_( StringHash( target ) );	// hash id for quick look up
				
				LibLoader::printExecutionMessage( std::string( "\t\tProcessing Instance Rigid Body: " ) + body );	
				LibLoader::printExecutionMessage( std::string( "\t\t\tTarget: " ) + target );
				
				// the rigid body instance transform is bound to the target uri
				// so we need to resolve the the target uri and get its transform.  
				// <rant> this is what i do not like about COLLADA.  The COLLADA DOM should be
				// doing this work for us. </rant>
				daeElementRef elem = pDomInstanceRigidBody->getTarget().getElement();
				domNodeRef node = *(domNodeRef*)&elem;
				domMatrix_Array& matrixArray		= node->getMatrix_array();
				domRotate_Array& rotateArray		= node->getRotate_array();
				domTranslate_Array& translateArray	= node->getTranslate_array();
				domScale_Array& scaleArray			= node->getScale_array();
				
				Matrix3f transform_basis;
				Vector3f transform_origin;
				Vector3f transform_scale(1.0f, 1.0f, 1.0f);
				
				//either load the matrix (worldspace) or incrementally build the transform from 'translate'/'rotate'
				for (int k = 0; k < matrixArray.getCount(); k++ )
				{
					domMatrixRef matrixRef = matrixArray[i];
					domFloat4x4 fl16 = matrixRef->getValue();
					Vector3f origin(fl16.get(3),fl16.get(7),fl16.get(11));
					Matrix3f basis(fl16.get(0),fl16.get(1),fl16.get(2),
									  fl16.get(4),fl16.get(5),fl16.get(6),
									  fl16.get(8),fl16.get(9),fl16.get(10));
					transform_basis = basis;
					transform_origin = origin;
				}
				
				for (int k = 0; k < rotateArray.getCount(); k++ )
				{
					domRotateRef rotateRef = rotateArray[k];
					domFloat4 fl4 = rotateRef->getValue();
					float angleRad = Mathf::DEG_TO_RAD*fl4.get(3);
					Quaternionf rotQuat( Vector3f( fl4.get(0),fl4.get(1),fl4.get(2) ), angleRad);	// axis angle to quat
					Matrix3f rotMat;
					rotQuat.ToRotationMatrix( rotMat );
					transform_basis = transform_basis * rotMat;
				}
				
				for ( int k = 0; k < translateArray.getCount(); k++ )
				{
					domTranslateRef translateRef = translateArray[k];
					domFloat3 fl3 = translateRef->getValue();
					Vector3f orgTrans(fl3.get(0),fl3.get(1),fl3.get(2));
					transform_origin = transform_origin + orgTrans;
				}
				
				for( int k = 0; k < scaleArray.getCount(); k++ )
				{
					domScaleRef scale = scaleArray[k];
					domFloat3 fl3 = scale->getValue();
					transform_scale[0] = fl3.get(0);
					transform_scale[1] = fl3.get(1);
					transform_scale[2] = fl3.get(2);
					
					LibLoader::printExecutionMessage( "***PROCESSING SCALE***" );
															
				}
				
				vec3f *t = pPbInstanceRigidBody->mutable_position_();
				t->set_x( transform_origin[0] );
				t->set_y( transform_origin[1] );
				t->set_z( transform_origin[2] );
				
				Quaternionf rot;
				rot.FromRotationMatrix( transform_basis );
				vec4f *r = pPbInstanceRigidBody->mutable_rotation_();
				r->set_x( rot[0] );
				r->set_y( rot[1] );
				r->set_z( rot[2] );
				r->set_w( rot[3] );
				
				vec3f *s = pPbInstanceRigidBody->mutable_scale_();
				s->set_x( transform_scale[0] );
				s->set_y( transform_scale[1] );
				s->set_z( transform_scale[2] );	
																			
			}	// for each rigid body instance
			
		}	// for each physics model instance
		
	} // for each physics scene
			
	return true;
}

//Register your conditioner so the Refinery knows about it, and how to spawn it.
Conditioner::Register<MyConditionerProxy > myConditionerProxy;